/*LICENSE_BEGIN
 * ******************************************************************
 * Copyright 2006 by the Dainamite Robocup-Team at the
 * DAI-Labor, TU-Berlin (www.dai-labor.de)
 * 
 * Members of Dainamite are:
 * Holger Endert
 * Robert Wetzker
 * Thomas Karbe
 * Axel Heßler
 * Philippe Bütter
 * Felix Brossmann
 * 
 * Licensed under the GNU GPL (General Public License) Free Software 
 * License, Version 2. You may use this file only in compliance with 
 * the License. You may obtain a copy of the License at:
 * 
 * http://www.fsf.org/licensing/licenses/gpl.txt
 * ******************************************************************
LICENSE_END*/
/**
 * File     MCLFilter.java
 * Package  robocup.component.worldmodel
 * Project  Robocup
 * Date     Jun 18, 2005
 * Web      www.dai-labor.de
 * 
 * @author  Grzegorz Lehmann
 */
package robocup.component.filters;

import java.util.Random;

import robocup.component.geometry.Vektor;

/**
 * <code>MCLFilter</code> used for calculating the players
 * position based on the flags and lines seen.
 * 
 * this filter is less accurate but more efficient then the other and
 * therefore used only when the ball is far.
 * 
 * @author Grzegorz Lehmann
 */
public class MCLFilter {
	private static final int MAX_PARTICLES = 50;

	private Particle[] xt = new Particle[MAX_PARTICLES];

	private Particle[] xt_1 = new Particle[MAX_PARTICLES];

	private Particle ut = new Particle(new Vektor(), .9, 0.);

	private Particle ut_1 = new Particle(new Vektor(), .9, 0.);

	/**
	 * <code>MAX_PARTICLE_DIFF</code>
	 */
	private static final double MAX_PARTICLE_DIFF = .4;

	/**
	 * <code>MAX_MEASUREMENT_DIST</code>
	 */
	private static final double MAX_MEASUREMENT_DIST = 60;

	/**
	 * <code>MAX_U_LENGTH</code>
	 */
	private static final double MAX_U_LENGTH = 2.2;

	/**
	 * <code>RANDOM_SAMPLES</code>
	 */
	private static final int RANDOM_SAMPLES = 10;

	private int xts = 0;

	private int xt_1s = 0;

	/**
	 * <code>MCLFilter</code> constructor
	 */
	public MCLFilter() {
		reset(new Vektor());
	}

	/**
	 * <code>sampleParticle</code>
	 * 
	 * @param position
	 *            the measured position
	 * @param distance
	 *            distance of the measurement
	 * @return double - probability
	 */
	public final double sampleParticle(Vektor position, double distance) {
		// System.out.println(position.toString());
		double p;
		if (this.xt_1s < 1) {
			p = 1.;
		} else {
			p = 0.;
			// resample particles t-1 with new position and sum
			for (int i = 0; i < this.xt_1s; i++) {
				p += sampleParticle(position, this.xt_1[i]);
			}
		}
		// probability of the measurement
		p *= p_y_x(distance);
		// add new particle
		if (this.xts >= this.xt.length) {
			// particle array full, replacing the oldest particle
			this.xt[0] = new Particle(position, p, distance);
		} else {
			this.xt[this.xts++] = new Particle(position, p, distance);
		}
		return p;
	}

	/**
	 * <code>sampleParticle</code>
	 * 
	 * @param position
	 *            the measured position at time t
	 * @param particle
	 *            a particle at time t-1
	 * @return double - probability
	 */
	private final double sampleParticle(Vektor position, Particle particle) {
		// sum component
		return getWeight(particle) * p(position, particle, this.ut)
				* particle.getProbability();
	}

	/**
	 * <code>setU</code> sets a new odemetry data u, i.e. the new movement
	 * vector. This also starts a new time cycle.
	 * 
	 * @param u
	 *            odometry measurement
	 */
	public final void setU(Vektor u) {
		if (u == null) {
			return;
		}
		double max = 0.;
		double temp;
		for (int i = 0; i < this.xts; i++) {
			temp = this.xt[i].getProbability();
			if (temp > max) {
				max = temp;
			}
		}
		for (int i = 0; i < this.xts; i++) {
			this.xt[i].scale(max);
		}
		this.ut_1 = this.ut;
		this.ut = new Particle(u, p_y_u(u), 0.);
		System.arraycopy(this.xt, 0, this.xt_1, 0, this.xts);
		this.xt_1s = this.xts;
		this.xts = 0;
	}

	/**
	 * <code>sampleRandom</code>
	 * 
	 */
	public final void sampleRandom() {
		Random rand = new Random();
		Vektor pos = getPosition();
		Vektor temp;
		final double dev = .3;
		for (int i = 0; i < RANDOM_SAMPLES; i++) {
			temp = new Vektor(Vektor.XY, pos.x + dev * rand.nextGaussian()
					- dev / 2, pos.y + dev * rand.nextGaussian() - dev / 2);
			sampleParticle(temp, rand.nextGaussian() * MAX_MEASUREMENT_DIST);
		}
	}

	/**
	 * <code>p</code> motion model
	 * 
	 * @param x_t
	 *            particle at time t
	 * @param x_t_1
	 *            particle at time t-1
	 * @param u_t
	 *            action at time t-1 (before t)
	 * @return double - probability
	 */
	private final double p(Vektor x_t, Vektor x_t_1, Vektor u_t) {
		return p_dist(x_t_1.add(u_t).sub(x_t).getLength(), MAX_PARTICLE_DIFF);
	}

	/**
	 * <code>getWeight</code>
	 * 
	 * @param particle
	 *            particle to weight
	 * @return double - weight
	 */
	private final double getWeight(Particle particle) {
		return p_y_x(particle.getDistance()) / p_y_u(this.ut_1);
	}

	/**
	 * <code>p_y_u</code> returns the probability of u, .5 is minimum.
	 * 
	 * @param u
	 *            u vector
	 * @return double - likely value of u
	 */
	private final double p_y_u(Vektor u) {
		return (p_dist(u.getLength(), MAX_U_LENGTH));
	}

	/**
	 * <code>p_y_x</code>
	 * 
	 * @param distance
	 * @return double
	 */
	private final double p_y_x(double distance) {
		return p_dist(distance, MAX_MEASUREMENT_DIST);
	}

	/**
	 * <code>p_dist</code>
	 * 
	 * @param distance
	 * @param maximum
	 * @return double
	 */
	private final double p_dist(double distance, double maximum) {
		if (Math.abs(distance) >= maximum) {
			return .00000001;
		}
		// norm
		distance /= maximum;

		return 1 - Math.pow(distance, 2);
	}

	/**
	 * <code>reset</code>
	 * 
	 * @param position
	 */
	public final void reset(Vektor position) {
		if (position == null) {
			position = new Vektor();
		}
		this.ut = new Particle(new Vektor(), 1., 0.);
		this.ut_1 = new Particle(new Vektor(), 1., 0.);
		this.xt[0] = new Particle(position, 1., 0.);
		this.xt_1[0] = new Particle(position, 1., 0.);
		this.xt_1s = this.xts = 1;
	}

	/**
	 * <code>getPosition</code>
	 * 
	 * @return Vektor
	 */
	public Vektor getPosition() {
		if (this.xts < 1) {
			return null;
		}
		Particle particle = this.xt[0];
		double belief = particle.getProbability();
		Vektor v = particle.mult2(belief);
		double sum = belief;
		for (int i = 1; i < this.xts; i++) {
			particle = this.xt[i];
			belief = Math.sqrt(particle.getProbability());
			// belief *= belief;
			sum += belief;
			v.addToThis(particle.mult2(belief));
		}
		return (sum <= 0) ? v : v.div2(sum);
	}
}
